#include "stdafx.h"
#include "Matrix3d.h"
#include "Vector3d.h"
#include "GeomUtils.h"

using namespace stl;

void IdenticalMatrix(double matrix[4][4])
{
	for (int i = 0; i < 4; ++i)
	{
		double dValue = 0.0;
		for (int j = 0; j < 4; ++j)
		{
			if (i == j)
				dValue = 1.0;

			matrix[i][j] = dValue;
		}
	}
}

//////////////////////////////////////////////////////////////

CMatrix3D::CMatrix3D()
{
	//IdenticalMatrix(A);
}

CMatrix3D::CMatrix3D(const MATRIX3d & matrix)
{
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; ++j)
		{
			A[i][j] = matrix.A[i][j];
		}
	}
}

CMatrix3D::CMatrix3D(const double * pMatrix)
{
	//IdenticalMatrix(A);
	if (pMatrix != nullptr)
	{
		for (int i = 0; i < 4; i++)
		{
			for (int j = 0; j < 4; ++j)
			{
				A[i][j] = pMatrix[i * 4 + j];
			}
		}
	}
}

CMatrix3D::~CMatrix3D()
{
}

CMatrix3D CMatrix3D::operator*(const MATRIX3d & matrix2) const
{
	CMatrix3D matrix;
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			matrix.A[i][j] = A[i][0] * matrix2.A[0][j]
				+ A[i][1] * matrix2.A[1][j]
				+ A[i][2] * matrix2.A[2][j]
				+ A[i][3] * matrix2.A[3][j];
		}
	}
	return matrix;
}

void CMatrix3D::operator*=(const MATRIX3d & matrix)
{
	(*this) = (*this)*matrix;
}

void CMatrix3D::IdeticalMatrix()
{
	IdenticalMatrix(A);
}

double CMatrix3D::GetValue() const
{
	return  A[0][0] * A[1][1] * A[2][2] +
		A[0][1] * A[1][2] * A[2][0] +
		A[0][2] * A[1][0] * A[2][1] -
		A[0][2] * A[1][1] * A[2][0] -
		A[0][1] * A[1][0] * A[2][2] -
		A[0][0] * A[1][2] * A[2][1];
}

double CMatrix3D::GetValue(double a00, double a01, double a02,
	double a10, double a11, double a12, double a20, double a21, double a22)
{
	return  a00 * a11*a22 +
		a01 * a12*a20 +
		a02 * a10*a21 -
		a02 * a11*a20 -
		a01 * a10*a22 -
		a00 * a12*a21;
}

CMatrix3D CMatrix3D::CreateMirrorMatrix(VECTOR3D v)
{
	double len = ((CVector3D)v).GetLength();
	CMatrix3D matrix;
	matrix.A[0][0] = (v.x*v.x - 1.0)*2.0 / len / len;
	matrix.A[1][1] = (v.y*v.y - 1.0)*2.0 / len / len;
	matrix.A[2][2] = (v.z*v.z - 1.0)*2.0 / len / len;
	matrix.A[0][1] = matrix.A[1][0] = v.x*v.y*2.0 / len / len;
	matrix.A[0][2] = matrix.A[2][0] = v.x*v.z*2.0 / len / len;
	matrix.A[1][2] = matrix.A[2][1] = v.z*v.y*2.0 / len / len;
	return matrix;
}

CMatrix3D CMatrix3D::CreateRotateMatrix(double da, VECTOR3D v)
{
	CMatrix3D R;
	CVector3D bv(v);

	if (GeIsZero(da))	return R;

	double lxy = bv.GetLengthXY();
	if (GeIsZero(lxy))
	{
		if (bv.z < 0.0) da *= -1.0;
		R.A[0][0] = R.A[1][1] = cos(da);
		R.A[0][1] = sin(da); R.A[1][0] = -sin(da);
		return R;
	}
	double lyz = bv.GetLengthYZ();
	if (GeIsZero(lyz))
	{
		if (bv.x < 0.0) da *= -1.0;
		R.A[2][2] = R.A[1][1] = cos(da);
		R.A[1][2] = sin(da); R.A[2][1] = -sin(da);
		return R;
	}
	double lxz = bv.GetLengthZX();
	if (GeIsZero(lxz))
	{
		if (bv.y < 0.0) da *= -1.0;
		R.A[0][0] = R.A[2][2] = cos(da);
		R.A[0][2] = -sin(da); R.A[2][0] = sin(da);
		return R;
	}

	CMatrix3D Rz;
	Rz.A[0][0] = Rz.A[1][1] = bv.y / lxy;
	Rz.A[0][1] = bv.x / lxy; Rz.A[1][0] = -bv.x / lxy;

	double len = bv.GetLength();
	CMatrix3D Rx;
	Rx.A[2][2] = Rx.A[1][1] = bv.z / len;
	Rx.A[1][2] = lxy / len; Rx.A[2][1] = -lxy / len;

	R.A[0][0] = R.A[1][1] = cos(da);
	R.A[0][1] = sin(da); R.A[1][0] = -sin(da);

	CMatrix3D Rxn;
	Rxn.A[2][2] = Rxn.A[1][1] = bv.z / len;
	Rxn.A[2][1] = lxy / len; Rxn.A[1][2] = -lxy / len;

	CMatrix3D Rzn;
	Rzn.A[0][0] = Rzn.A[1][1] = bv.y / lxy;
	Rzn.A[1][0] = bv.x / lxy; Rzn.A[0][1] = -bv.x / lxy;

	return Rz * Rx*R*Rxn*Rzn;
}

CMatrix3D CMatrix3D::CreateScaleMatrix(double dScale)
{
	CMatrix3D m;
	m.A[0][0] = m.A[1][1] = m.A[2][2] = dScale;
	return m;
}

CMatrix3D CMatrix3D::CreateTransfMatrix(VECTOR3D vec)
{
	CMatrix3D m;
	m.A[3][0] = vec.x;
	m.A[3][1] = vec.y;
	m.A[3][2] = vec.z;
	return m;
}
